#include "uni_motor.h"

#define BORD_RATE 115200//设定串口波特率为115200
UNI_MOTOR motor1(1, true);//定义编码电机1
UNI_MOTOR motor2(2, true);//定义编码电机2
UNI_MOTOR motor3(3, true);//定义编码电机3
UNI_MOTOR motor4(4, true);//定义编码电机4
int stepper_speed = 4800; //设定步进电机速度

void setup() {
  delay(500);//开机等待500毫秒
  Serial.begin(BORD_RATE);//打开串口
  stepper_init();//步进电机引脚初始化
  car_init();//麦克纳姆四轮小车四个电机引脚初始化
  stepmove_test();//步进电机运动测试
  //car_action_test(); //小车动作测试
}

void loop() {
  car_update(); //实时更新并保持小车四个电机的位置
}

//步进电机运动测试
void stepmove_test(){
  move_distance(36);  //机械导轨向上移动36毫米
  delay(3000);
  move_distance(-36); //机械导轨向下移动36毫米
  delay(3000);
}

//小车动作测试
void car_action_test(){
  car_move_distance( 0.2, 0.0, 0.0);   //小车前进20厘米
  car_move_distance(-0.2, 0.0, 0.0);   //小车后退20厘米
  car_move_distance(0.0,  0.2, 0.0);   //小车左平移20厘米
  car_move_distance(0.0, -0.2, 0.0);   //小车右平移20厘米
  car_move_distance(0.0,  0.0, 90.0);  //小车左转90度
  car_move_distance(0.0,  0.0, -90.0); //小车右转90度
}